#include "stm.h"
#include <math.h>
#include <time.h>
#include "graphbmp-tools.h"
#include "los.h"
#define BARS
/** valore speciale per dire che nessuna tipologia di mossa è taboo. */
#define TABOO_MOVE_NOT_SET -1

/** Livello di verbosità dell'iterazione STM.
 * \li 0 non stampa nulla eccetto errori.
 * \li 1 stampa una breve didascalia per ogni iterazione LOS, per ogni veicolo.
 * \li 2 stampa uno snapshot dello stato, delle risorse rimanenti, della taboo, della raccolta per ogni iterazione LOS, per ogni veicolo. */
#define VERBOSE 0

pSTMConfiguration initSTMConfiguration(PalgConfiguration pALGconfig){
	pSTMConfiguration result = malloc(sizeof(STMConfiguration));
	checkMalloc(result, "stm.h:initSTMConfiguration:1");
	result->drawSolutionBMP = pALGconfig->drawSolutionBMP;
	result->drawSolutionOctave = pALGconfig->drawSolutionOctave;
	result->maximumLOSIterationNumber = pALGconfig->maximumLOSIterationNumber;
	result->maximumLOSWorseningIterationNumber = pALGconfig->maximumLOSWorseningIterationNumber;
	//TODO ricordarsi di deallocare questa struttura dalla memoria per evitare il memoryleak
	result->pLOSconfig = initLOSConfiguration(pALGconfig);

	return result;
}

void initSTMEngine(int T, int Q){
	//inizializzo la matrice di covarianza
	stmCovariance[0][0]=1;
	stmCovariance[0][1]=0;
	stmCovariance[1][0]=0;
	stmCovariance[1][1]=1;
	//calcolo il determinante della matrice di covarianza
	stmCovarianceDet=(stmCovariance[0][0]*stmCovariance[1][1]-stmCovariance[1][0]*stmCovariance[0][1]);
	//calcolo l'inversa della matrice di covarianza
	if (!stmCovarianceDet)
		fprintf(stderr,"stmCovarianceDet uguale a 0!\n");

	stmCovarianceInv[0][0] = (stmCovariance[1][1])/(stmCovarianceDet);
	stmCovarianceInv[0][1] = (stmCovariance[0][1])/(-stmCovarianceDet);
	stmCovarianceInv[1][0] = (stmCovariance[1][0])/(-stmCovarianceDet);
	stmCovarianceInv[1][1] = (stmCovariance[0][0])/(stmCovarianceDet);
	//calcolo il coefficiente amplificativo della gaussiana
	//stmGaussianAmplifier=1/(4*M_PI*M_PI*sqrtf(stmCovarianceDet));
	stmGaussianAmplifier=1;
	//TODO impostare il parametro alpha
	stmMinProbabilisticValue=0.01;
	stmGaussianLogBase=(-2*logf(stmMinProbabilisticValue))/
						(T*T*stmCovarianceInv[0][0]+T*Q*stmCovarianceInv[1][0]+T*Q*stmCovarianceInv[0][1]+Q*Q*stmCovarianceInv[1][1]);
	//inizializzo la tabooList per le mosse
	/*int i;
	for (i=0;i<STM_MOVES_AVAIABLE;i++){
		moveTabooList[i]=0lu;
	}*/
	//effettuo il seed del generatore casuale di numeri assicurandomi che venga chiamato una sola volta
	randomize();
}

Psolution executeSTM(Psolution p_solution, Pgraph p_graph, PtabooState p_taboostate,int* totalIteration, int* solutionintensity,pSTMConfiguration pSTMconfig){

	int k; //indice del camion generico
	FILE* fObjHistory;
	FILE* movesHistory;

	if(pSTMconfig->drawSolutionOctave)
		initializeOutputScripts(&fObjHistory, &movesHistory, *p_solution->K, p_graph->T, p_graph->Q, p_solution);

	TabooExpire currentIteration; //identifica l'iterazione LOS attuale
	TabooExpire lastIncrementingIteration; //indica l'ultima iterazione in cui si ha avuto una modifica alla migliore soluzione
	/* TODO proposta di struttura copiata (non referenziata) per contenere ogni volta che il profitto migliora
		(localmente a livello veicolo => globalmente per l'indipendenza dei veicoli)
		1- mallocare e inizializzare
		2- ogni volta che il k-esimo vehicleState in esame migliora rispetto a bestEverSolution, salvarlo in essa
		3- alla fine di stm se pbestEverSolution è migliore di solution, restituire riferimento ad essa
			(o più correttamente clonarla in psolution e deallocarla) */
	Psolution pbestEverSolution=NULL; //indica la soluzione migliore
	Psolution pStartingSolution=cloneSolution(p_solution); //indica una copia della soluzione iniziale da ritornare solo in caso LOS peggiori o fallisca sempre
	int i; //variabile di ciclo

	currentIteration = 1; // si considera iterazione 0 la costruttiva. (patTest è stato testato così)
	lastIncrementingIteration = 1; // conseguentemente
	//costruisco i punti dove le 3 guassiano sono massime
	VectorParam averageAdd = {0,0};
	VectorParam averageSwap = {p_graph->T/2, p_graph->Q/2};
	VectorParam averageRemove = {p_graph->T, p_graph->Q};
	VectorParam nowTQ;
	int moveToApply; //mossa da inviare al LOS
	char str[50]; //nome del file di soluzione
	int LOSexitStatus; // flag di ritorno per i solveXneighbourhood()
	PvehicleState pvs; //riferimento per brevità
	PtabooList ptl; //riferimento per brevità

	/** taboo basica sulle mosse. Qualora una mossa di un certo tipo fallisca, dev'essere vietata finchè viene estratta continuamente lei stessa!
	 * vale TABOO_MOVE_NOT_SET xor il valore associato alla macro MOVE_XXX. */
	int tabooMove = TABOO_MOVE_NOT_SET;

	while (stmMustContinue(currentIteration, pSTMconfig->maximumLOSIterationNumber,
						   lastIncrementingIteration, pSTMconfig->maximumLOSWorseningIterationNumber)){
		#if VERBOSE > 0
			printf("\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
		#endif

		for (k=0; k<*p_solution->K; k++){

			pvs = getVehicleStateInSolution(p_solution, k);
			ptl = getTabooListInTabooState(p_taboostate, k);

			nowTQ.x1 = pvs->tau;
			nowTQ.x2 = pvs->theta;
			//printf("sto giro mo")
			do {
				moveToApply = selectBestLOSMove(&nowTQ, &averageAdd, &averageSwap, &averageRemove);
			} while(moveToApply==tabooMove);
			tabooMove = TABOO_MOVE_NOT_SET; // ho cambiato mossa, effettuo lo sblocco
			LOSexitStatus = solveLocalSearch(p_graph, p_solution, k, p_taboostate, moveToApply, currentIteration, pSTMconfig->pLOSconfig);
			if (LOSexitStatus==MOVE_STATUS_IMPROVE || LOSexitStatus==MOVE_STATUS_UNCHANGED) {
				//TODO molto poco performante... magari si potrebbe semplicemente modificare la soluzione invece che disporla e clonarla
				lastIncrementingIteration = currentIteration;
				if (pbestEverSolution!=NULL)
					freeSolution(pbestEverSolution);
				pbestEverSolution = cloneSolution(p_solution);
			}
			if(LOSexitStatus==MOVE_STATUS_FAIL)
				tabooMove = moveToApply;

			#if VERBOSE > 0
				printf("\nTerminata mossa di tipo ");
				if(moveToApply==MOVE_ADD)
					printf("ADD | ");
				else if(moveToApply==MOVE_SWAP)
					printf("SWAP | ");
				else
					printf("REMOVE | ");
				printf("iterazione %lu, ultima con successo %lu, veicolo %d.\n", currentIteration, lastIncrementingIteration, k+1);
				if(LOSexitStatus==MOVE_STATUS_IMPROVE)
					printf("la soluzione è MIGLIORATA\n");
				else if(LOSexitStatus==MOVE_STATUS_WORSEN)
					printf("la soluzione è PEGGIORATA\n");
				else if(LOSexitStatus==MOVE_STATUS_UNCHANGED)
					printf("la soluzione è INVARIATA\n");
				else
					printf("la mossa non è stata eseguita perchè le risorse sono finite.\n");

				printVehicleTQJScalars(stdout, pvs);
				printRemainingResources(stdout, p_graph, pvs->tau, pvs->theta);
				#if VERBOSE > 1
					if(LOSexitStatus!=MOVE_STATUS_UNCHANGED) {
						printVehicleState(stdout, pvs, k);
						printVehicleGathering(stdout, pvs->pvp, p_solution->gMatrix, k);
						printTabooListAsSet(stdout, ptl, currentIteration, k);
					}
				#endif
			#endif

			if(pSTMconfig->drawSolutionOctave)
				updateOutputScripts(fObjHistory, movesHistory, k, p_graph->T, p_graph->Q, moveToApply, LOSexitStatus, pvs, currentIteration);
		}
		//disegno, se richiesta, l'immagine
		if (pSTMconfig->drawSolutionBMP){
			sprintf(str, STM_PICTURE_PATTERNNAME, currentIteration);
			drawSolutionFromScratchDefaults(str, p_solution, p_graph->depot);
		}
		//incremento il contatore iterazionemoveToApply
		currentIteration++;
	}
	currentIteration--; //sono sicuramente andato avanti di una!

	if(pSTMconfig->drawSolutionOctave)
		finalizeOutputScripts(fObjHistory, movesHistory, *p_solution->K, currentIteration);
	if (totalIteration!=NULL){
		*totalIteration=currentIteration;
	}
	if (solutionintensity!=NULL){
		*solutionintensity=currentIteration-lastIncrementingIteration;
	}
	if (pbestEverSolution!=NULL){
		//dealloco la soluzione iniziali in quanto obsoleta
		freeSolution(pStartingSolution);
		/*ora la soluzione puntata da p_solution deve essere deallocata in quanto non significa piu' nulla. Tuttavia il puntatore deve rimanere lo stesso
		 *in modo da poter puntare pbestEverSolution in modo tale che da fuori da questa funzione chi richiama psolution puo' comunque accedere alla
		 *soluzione migliore*/
		//dealloco la gMatrix di p_solution e ridirigo verso quella di pbestEverSolution
		freeGMatrix(p_solution->gMatrix);
		p_solution->gMatrix=pbestEverSolution->gMatrix;
		//dealloco tutto il vehicleState di p_solution e associo i percorsi di pbesrEverSolution
		for (i=0;i<*p_solution->K;i++){
			freeVehicleState(p_solution->states[i]);
			p_solution->states[i]=pbestEverSolution->states[i];
		}
		p_solution->K=pbestEverSolution->K;
		return pbestEverSolution;
	}else{
		//questa STM ha sempre peggiorato la soluzione
		return pStartingSolution;
	}

}

bool stmMustContinue(TabooExpire iterationNumber,TabooExpire maxIterationNumber,
					 TabooExpire lastIncrementingIteration, TabooExpire maxContigousDecrementingIteration){
	//printf("continuo sse %lu < %lu && ( %lu - %lu ) < %lu\n", iterationNumber, maxIterationNumber, iterationNumber, lastIncrementingIteration, maxContigousDecrementingIteration);
	return iterationNumber < maxIterationNumber && (iterationNumber - lastIncrementingIteration) < maxContigousDecrementingIteration;
}

// ~~~~~ MATLAB/OCTAVE SCRIPTS ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void initializeOutputScripts(FILE** pfObjHistory, FILE** pmovesHistory, int K, int T, int Q, Psolution ps) {
	int k;

	*pfObjHistory = fopen(STM_MATLAB_F_OBJ_HISTORY, "w");
	FILE* file = *pfObjHistory;
	//fprintf(fObjHistory, "clear, clc, close all; X = zeros(%lu,3,%d);\n", itSTM+1, __K__);
	/* NON SO il numero finale di iterazioniSTM => non so la dimensione effettiva della matrice X.
	   fortunatamente per matlab/octave questo non è un problema :D */
	fprintf(file, "clear, clc, close all\n");

	for(k=0; k<K	; k++) { // stampa dello stato dopo la costruttiva
		fprintf(file, "X(1,1,%d) = %d; ",  k+1, T - getVehicleStateInSolution(ps, k)->tau);
		fprintf(file, "X(1,2,%d) = %d; ",  k+1, Q - getVehicleStateInSolution(ps, k)->theta);
		fprintf(file, "X(1,3,%d) = %d;\n", k+1,     getVehicleStateInSolution(ps, k)->jk);
	}

	*pmovesHistory = fopen(STM_MATLAB_MOVES_HISTORY, "w");
	file = *pmovesHistory;
	fprintf(file, "clear, clc, close all\n");
}

void updateOutputScripts(FILE* fObjHistory, FILE* movesHistory, int k, int T, int Q, int moveToApply, int LOSExitStatus,
							PvehicleState pvs, TabooExpire currentIteration) {
	// matlab indicizza da 1, ricordarsi..
	fprintf(fObjHistory, "X(%lu,1,%d) = %d; ",  currentIteration+1, k+1, T - pvs->tau);
	fprintf(fObjHistory, "X(%lu,2,%d) = %d; ",  currentIteration+1, k+1, Q - pvs->theta);
	fprintf(fObjHistory, "X(%lu,3,%d) = %d;\n", currentIteration+1, k+1,     pvs->jk);
	fprintf(movesHistory, "X(%lu,%d,1) = %d;\n", currentIteration, k+1, moveToApply+1);
	fprintf(movesHistory, "X(%lu,%d,2) = %d;\n", currentIteration, k+1, LOSExitStatus+1);
}

void finalizeOutputScripts(FILE* fObjHistory, FILE* movesHistory, int K, TabooExpire finalIteration) {
	int k;
	for(k=0; k<K; k++) {

		fprintf(fObjHistory, "\nfigure('name', 'VehicleState%d historical')\n", k+1);
		fprintf(fObjHistory, "plot(0:%lu, X(:,1,%d),'r'), hold on\n", finalIteration, k+1);
		fprintf(fObjHistory, "plot(0:%lu, X(:,2,%d),'g')\n", finalIteration, k+1);
		fprintf(fObjHistory, "plot(0:%lu, X(:,3,%d),'b')\n", finalIteration, k+1);
		fprintf(fObjHistory, "legend('T-\\tau','Q-\\theta','J_k'), axis tight\n");
	}
	fclose(fObjHistory);

	for(k=0; k<K; k++) {

		fprintf(movesHistory, "\nfigure('name', 'Vehicle%d historical moves')\n", k+1);
		fprintf(movesHistory, "title('ADD=1; SWAP=2; REMOVE=3'), ");
		#ifdef BARS
			fprintf(movesHistory, "bar1 = bar(1:%lu, X(:,%d,1),'FaceColor','b','EdgeColor','b'); axis tight\n", finalIteration, k+1);
			fprintf(movesHistory, "set(bar1,'BarWidth', 0.5); hold on\n");
			fprintf(movesHistory, "bar2 = bar(1:%lu, X(:,%d,2),'FaceColor','r','EdgeColor','r');\n", finalIteration, k+1);
			fprintf(movesHistory, "set(bar2,'BarWidth', 0.2); legend('add/swap/remove','fail/worsen/improved')\n");
		#else
			fprintf(movesHistory, "plot(1:%lu, X(:,%d,1),'b'), hold on\n", finalIteration, k+1);
			fprintf(movesHistory, "plot(1:%lu, X(:,%d,2),'r'), axis tight\n", finalIteration, k+1);
		#endif
		fprintf(movesHistory, "\nfigure('name','Vehicle%d histogram of moves')\n", k+1);
		fprintf(movesHistory, "\nhist(X(:,%d,1))\n", k+1);
	}
	fclose(movesHistory);
}

// ~~~~~ GAUSSIAN SECTION ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

int selectBestLOSMove(PvectorParam x, PvectorParam averageAdd, PvectorParam averageSwap, PvectorParam averageRemove){

	float alpha_add = computeGaussian(computeDifference(x, averageAdd));
	float alpha_swap = computeGaussian(computeDifference(x, averageSwap));
	float alpha_remove = computeGaussian(computeDifference(x, averageRemove));
	//float range[3]={0.0f,alpha_add,alpha_swap,alpha_remove};
	//ora devo trasformarli in probabilita' e inserirli in unintervallo tra [0;1[
	float sum_alpha=alpha_add+alpha_remove+alpha_swap;

	float alpha_prob_add    =     0           +	alpha_add/sum_alpha;
	float alpha_prob_swap   = alpha_prob_add  +	alpha_swap/sum_alpha;
	float alpha_prob_remove = alpha_prob_swap + alpha_remove/sum_alpha;
	//ora estraggo un alpha randomico (il valore e' compreso tra [0;1[ )
	float r = ((float)rand()/(float)RAND_MAX);
	#if VERBOSE > 2
		printf("\nSelezione tra i range add: %f swap: %f remove %f | random: %f\n", alpha_prob_add, alpha_swap/sum_alpha, alpha_remove/sum_alpha, (float)r);
	#endif

	if (r <= alpha_prob_add)
		return MOVE_ADD;
	if (r <= alpha_prob_swap)
		return MOVE_SWAP;
	if (r <= alpha_prob_remove)
		return MOVE_REMOVE;

	fprintf(stderr,"Nessuna mossa scelta da STM in questa iterazione, per qualche errore. Provo ADD.\n");
	fprintf(stderr,"\nx= ");printVectorParam(stderr,x);
	fprintf(stderr,"exponent= %f",computeExponent(x));
	return MOVE_ADD;
}

float getGaussianAmplifier(){
	return stmGaussianAmplifier;
}

float computeExponent(PvectorParam v){
	return -0.5f*stmGaussianLogBase*(
			(v->x1*v->x1*stmCovarianceInv[0][0])+(v->x1*v->x2*stmCovarianceInv[1][0]) +
			(v->x1*v->x2*stmCovarianceInv[0][1])+(v->x2*v->x2*stmCovarianceInv[1][1]));
}

float computeGaussian(PvectorParam x){
	return stmGaussianAmplifier*expf(computeExponent(x));
}

